/* Copyright(C): Leaf Johnson 2009,2010

    This file is part of leaf-lib.

    leaf-lib is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    leaf-lib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with leaf-lib.  If not, see <http://www.gnu.org/licenses/>.


 * 
 * 
 */


#include <devices_ext/mti.h>
#include <stdarg.h>

#define MTI_PREAMBLE (uint8_t)(0xFA)
#define MTI_ID_OFF 0
#define MTI_MID_OFF 1
#define MTI_SZ_OFF 2
#define MTI_DATA_OFF 3
#define MTI_ID( m ) ((m)->body[MTI_ID_OFF])
#define MTI_MID( m ) ((m)->body[MTI_MID_OFF])
#define MTI_SZ( m ) ((m)->body[MTI_SZ_OFF])

/* 计算校验位 */
static uint8_t calc_cb( const MtiMsg *msg )
{
	uint8_t cb = 0;
	size_t sz = mtim_get_size( msg ) + MTI_DATA_OFF;
	size_t i;
	for( i = 0; i < sz; ++i ){
		cb += msg->body[i];
	}
	return 0-cb;
}

/* 发消息，自动添加校验位 */
int mti_send( MtiPort port, const MtiMsg *msg )
{
	uint8_t preamble = MTI_PREAMBLE;
	mti_write( port, &preamble, 1 );
	mti_write( port, msg->body, mtim_get_bodysize(msg) );
	uint8_t cb = calc_cb( msg );
	mti_write( port, &cb, 1 );
	return 0;
}

/* 收消息，返回正数表示消息id, 返回负数表示出错，错误号在上面有定义 */
int mti_recv( MtiPort port, MtiMsg *msg )
{
	uint8_t preamble = 0;
	size_t sz = 0;
	uint8_t *data = msg->body + MTI_DATA_OFF;
	int rd;
	/* 接收消息头 */
	while( ( rd = mti_read( port, &preamble, 1 ) ) == 1 ){
		if( preamble == MTI_PREAMBLE )
			break;
	}
	if( rd < 1 ) return MTI_ERR_COMM;

	/* 接收消息ID与MID */
	rd = mti_read( port, msg->body, 3 );
	if( rd < 2 ) return MTI_ERR_COMM;

	/* 获取消息长度 */
	bool sbuf = false;
	sz = MTI_SZ( msg );
	if( sz == 0xFF ){
		rd = mti_read( port, &sz, 2 );
		if( rd < 2 ) return MTI_ERR_COMM;
		if( sz > msg->capacity - 2 ) sbuf = true;
		data += 2;
	}else if( sz > msg->capacity ){
		sbuf = true;
	}

	/* 获取数据，如果消息的空间不足，只接收空间大小个消息 */
	rd = mti_read( port, data, sbuf ? msg->capacity : sz );
	if( sbuf ){
		mtim_set_size( msg, msg->capacity );
		return MTI_ERR_SBUF;
	}
	if( rd < sz ) return MTI_ERR_COMM;
	uint8_t cb;
	rd = mti_read( port, &cb, 1 );
	if( rd < 1 ) return MTI_ERR_COMM;
	mtim_set_size( msg, sz );
	if( cb != calc_cb(msg) ) return MTI_ERR_MSG;
	return mtim_get_mid( msg );
}

/* 从分配的内存空间中创建消息 */
MtiMsg *mti_create_msg( void *mem, size_t sz )
{
	if( sz < sizeof(MtiMsg) )
		return NULL;
	MtiMsg *msg;
	msg = (MtiMsg *) mem;
	msg->capacity = sz - sizeof(MtiMsg);
	return msg;
}

/* 消息设置 */
uint8_t mtim_get_id( const MtiMsg *msg )
{
	return MTI_ID( msg );
}
uint8_t mtim_get_mid( const MtiMsg *msg )
{
	return MTI_MID( msg );
}
size_t mtim_get_size( const MtiMsg *msg )
{
	const uint8_t *body = msg->body;
	size_t sz = body[MTI_SZ_OFF];
	if( sz == 0xFF ){
		sz = body[5];
		sz <<= 8;
		sz += body[4];
	}
	return sz;
}
size_t mtim_get_bodysize( const MtiMsg *msg )
{
	size_t ms = mtim_get_size( msg );
	if( ms < 0xFF )
		return ms + 1 + MTI_SZ_OFF;
	else
		return ms + 3 + MTI_SZ_OFF;
}
void mtim_set_id( MtiMsg *msg, uint8_t id )
{
	MTI_ID( msg ) = id;
}

void mtim_set_mid( MtiMsg *msg, uint8_t id )
{
	MTI_MID( msg ) = id;
}

void mtim_set_size( MtiMsg *msg, size_t sz )
{
	if( sz < 0xFF ){
		MTI_SZ(msg) = (uint8_t)sz;
	}else{
		uint8_t *data = &(msg->body[MTI_DATA_OFF]);
		MTI_SZ(msg) = 0xFF;
		data[0] = (uint8_t) sz;
		data[1] = sz >> 8;
	}
}

void mtim_set_data( MtiMsg *msg, size_t idx, uint8_t data )
{
	uint8_t *ary = msg->body;
	ary += ( (ary[MTI_SZ_OFF] == 0xFF) ? 2 : 0 ) + MTI_DATA_OFF;
	ary[idx] = data;
}

void mtim_make( MtiMsg *msg, uint8_t mid, size_t sz, ... )
{
	if( ( sz > msg->capacity) || ( sz >= 0xFF && sz > msg->capacity - 2 ) )
		return;
	mtim_set_mid( msg, mid );
	mtim_set_size( msg, sz );
	uint8_t *data = msg->body;
	data += ( ( sz >= 0xFF) ? 2 : 0 ) + MTI_DATA_OFF;
	size_t i;
	va_list params;
	va_start(params, sz);
	for( i = 0; i < sz; ++i ){
		data[i] = (uint8_t)va_arg( params, int );
	}
	va_end(params);
}

/* 数据获取，我只支持获取F1220格式的整数部分，下标表示第几个数据，编号从0开始
 * e.g. 一个三个欧拉角的数据消息中，前三个数据分别是roll, pitch, yaw
 * 那么有roll = mtim_get_int( msg, 0 );
 *  	pitch = mtim_get_int( msg, 1 );
 *  	yaw = mtim_get_int( msg, 2 );
 *  */
int mtim_get_int( MtiMsg *msg, size_t idx )
{
	if( idx * 4 + 4 > msg->capacity )
		return MTI_ERR_VALUE;
	uint8_t *data = msg->body;
	data += ( ( MTI_SZ(msg) == 0xFF) ? 2 : 0 ) + MTI_DATA_OFF + idx * 4;
	int raw;
	uint8_t *p = (uint8_t *)&raw;
	p[1] = data[0];
	p[0] = data[1];
	int val = raw;
	val >>= 4;
	if( raw & _BV(3) )
		val += ( ( val > 0 ) ? 1 : -1 );
	return val;
}

/* 调整一个整数，使之成为一个合法的角度(-180~+180) */
int  mti_adjust_angle( int angle )
{
	while( angle > 180 )
		angle -= 360;
	while( angle < -180 )
		angle += 360;
	return angle;
}
